/*
 * ConsoleInterface.cpp
 *
 *  Created on: 2021年12月25日
 *      Author: vicent-PC
 */

#include <unistd.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"
#include "driver/uart.h"
#include "string.h"
#include "driver/gpio.h"
#include "Custom_Config.h"

#include "cstm_gpio.h"
#include "stepper_motor.h"

#if 1 // 18138
#define CYCLES4_PER_STEP (4) //4 * 16
#define CYCLES8_PER_STEP (8) //8 * 16
#define TATAL_STEP (365)    //total cycles = 365 * 4
#define LINE_CNT (4)
#endif

#define ONE_STEP_SLEEP_MS (1)

gpio_num_t io_param_clkwise[4]={IO6,IO4,IO7,IO5}; //A+ -> B+ -> A- ->B-
gpio_num_t io_param_anticlkwise[4]={IO6,IO5,IO7,IO4}; // A+ -> B- -> A- -> B+

int set_io(int i, gpio_num_t* io_param, float speed = 1.0)
{
    for(int j=0; j<4; j++)
    {
       if(i%4==j)
       {
            set_gpio(io_param[j], 1);
       }else{
            set_gpio(io_param[j], 0);   
       }
    }
    vTaskDelay(ONE_STEP_SLEEP_MS/ portTICK_PERIOD_MS / speed);
    // usleep(600); // have test most quickly 400
    return 0;
}

int one_step4(float speed, gpio_num_t* io_param) {
    //todo
    for(int i=0;i < CYCLES4_PER_STEP;i++)								 
    {
        set_io(i, io_param, speed);
    }
    return 0;
}

int set_io8(int i, gpio_num_t* io_param, float speed = 1.0)
{
    int step = i / 2;
    bool is_inner = (i % 2 != 0);
    for(int j = 0; j < LINE_CNT; j++)
    {
        if(step % 4 == j)
        {
            set_gpio(io_param[j], 1);

        } else if(is_inner) {
            set_gpio(io_param[(j+1)%LINE_CNT], 1);
        }else{
            set_gpio(io_param[j], 0);   
        }
    }
    vTaskDelay(ONE_STEP_SLEEP_MS/ portTICK_PERIOD_MS / speed);
    // usleep(500); // have test most quickly 400
    return 0;
}

int one_step8(float speed, gpio_num_t * io_param) {
    //todo
    for(int i=0;i < CYCLES8_PER_STEP;i++)								 
    {
        set_io8(i, io_param, speed);
    }
    return 0;
}

int one_step(float speed, bool clockwise = true) {
    // todo
    if (clockwise) {
        one_step4(speed, io_param_clkwise);
    } else {
        one_step4(speed, io_param_anticlkwise);
    }
    return 0;
}

// init motor
int stepper_init() {
    // gpio init
    init_gpio_output(IO4); // B+
    init_gpio_output(IO5); // B-
    init_gpio_output(IO6); // A+
    init_gpio_output(IO7); // A-

    // park to 0
	// stepper_run(TATAL_STEP, false);
    return 0;
}

int stepper_stop() {
    // todo
    for(int j=0; j<4; j++)
    {
        set_gpio(io_param_clkwise[j], 0);   
    }
    return 0;
}

// step from 0 to 1024
int stepper_run(int step, bool clockwise) {
    // todo
    float speed = 0.3; //speed 0.5 - 1.0
    for (int i = 0; i < step; i++) {
        // limit speed
        speed = (step - i) / (float)step;
        if (speed < 0.3) speed = 0.3;

        one_step(speed, clockwise);
    }
    stepper_stop();
    return 0;
}

